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ABSTRACT 


This  thesis  is  centered  upon  an  optimal  trajectory 
generation  algorithm  that  allows  real-time  control  for 
cooperation  of  multiple  quadrotor  vehicles  for 
intelligence,  surveillance,  and  reconnaissance  missions 
with  minimal  user  input.  The  algorithm  is  designed  for  an 
indoor  environment  where  global  positioning  system  data  is 
unavailable  or  unreliable,  forcing  the  vehicles  to  obtain 
position  data  using  other  sensors.  This  thesis  specifies 
the  lab  setup  and  well  as  the  control  approach  used.  Data 
acquired  from  two  experiments  is  included  to  demonstrate 
the  effectiveness  of  the  control  approach. 

The  control  approach  described  within  allows  for  a 
fully  autonomous  system  with  user  input  required  only  at 
the  initiation  of  a  mission.  The  algorithm  blends 
trajectory  planning,  trajectory  following,  and  multi¬ 
vehicle  coordination  to  achieve  the  goal  of  autonomy.  The 
focus  of  the  thesis  was  on  trajectory  generation  and  multi¬ 
vehicle  coordination,  while  leveraging  existing  trajectory 
following  controller  implementations.  The  trajectory 
generation  is  accomplished  with  a  direct  transcription  of 
the  optimization  problem  that  leverages  inverse  dynamics 
and  separates  spatial  and  temporal  planning.  The  vehicle 
motion  is  constrained,  and  simplifying  multi-vehicle 
coordination  assumptions  allow  for  the  efficient  solution 
and  execution  of  the  problem. 
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I.  INTRODUCTION  AND  OVERVIEW  OF  EXISTING 

APPROACHES 

A .  GENERAL 

An  unmanned  vehicle  (UV)  is  a  power-driven  vehicle 
that  does  not  carry  an  operator  and  is  either  remotely  or 
autonomously  controlled.  Unmanned  vehicles  can  perform  a 
wide  variety  of  missions  and  carry  a  vast  array  of 
payloads.  Most  are  designed  to  be  recovered  after  use  while 
others  are  designed  to  be  cheap  throwaway  alternatives. 

UVs  are  typically  broken  down  into  four  categories: 
unmanned  air  vehicles  (UAV) ,  unmanned  ground  vehicles 
(UGV) ,  unmanned  surface  vehicles  (USV) ,  and  unmanned 
underwater  vehicles  (UUV) .  There  has  been  a  recent 
explosion  in  the  number  and  variety  of  designs  for  each  of 
the  preceding  types  of  vehicles,  and  for  good  reason.  UVs 
are  perfect  for  performing  tasks  historically  done  by 
humans  that  are  dull,  dirty,  and  dangerous  [1] . 

Examples  where  UVs  are  replacing  humans  are 
intelligence  surveillance  and  reconnaissance  (ISR) 
missions,  collecting  signals  intelligence,  and  mapping 
ocean  floor  topographies.  Each  of  the  previous  missions 
fall  in  the  categories  of  dull,  dirty,  and  dangerous,  and 
replacing  a  human  with  an  unmanned  vehicle  improves  safety 
and  efficiency  in  each  case.  Instead  of  confining  a  pilot 
to  an  enclosed  cockpit  of  a  spy-plane  for  hours  on  end, 
where  fatigue  would  become  problematic  for  the  pilot,  an 
unmanned  air  vehicle  is  introduced  and  the  potential  for 
fatigue  is  reduced.  In  this  case,  operators  can  easily  be 
rotated  in  shifts  without  the  need  for  the  platform  to 
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return  to  an  airfield.  If  we  take  the  evolution  even 
further,  an  autonomous  vehicle  can  take  off,  perform  the 
mission,  and  then  return  to  its  airfield  with  little  to  no 
human  interaction  required. 

B .  VEHICLE  SELECTION 

When  selecting  a  type  of  UV  for  a  specific  mission, 
the  choices  can  be  daunting,  and  the  options  are  only 
expanding  with  the  recent  amplification  of  research  into 
the  field  of  unmanned  systems.  To  name  a  few  options,  there 
are  tracked,  wheeled,  and  legged  ground  vehicles  [2] -[4]. 
These  vehicles  are  accompanied  by  an  array  of  propeller 
driven  and  jet-drive  surface  craft  or  underwater  vehicles 
[5] -[6] .  In  addition,  one  could  choose  a  blimp,  fixed  wing 
or  rotary  aircraft  to  conduct  a  mission  [6] -[7]  .  Although 
each  type  of  vehicle  has  its  advantages,  the  quadrotor  is 
the  most  versatile,  substantiated  below. 

1 .  Quadrotor  Advantages 

a.  Hover  Capability 

Quadrotors  have  the  ability  to  hover  in  one 
location  for  an  extended  period  of  time.  This  hover  has 
several  consequences.  First,  a  hover  gives  the  user  a  great 
deal  of  maneuvering  flexibility  when  conducting  ISR 
missions.  The  hover  capability  also  allows  the  vehicle  to 
maneuver  in  a  physically  constrained  environment  such  as 
that  found  in  urban  areas.  The  ability  to  hover  enables  the 
vehicle  to  take  off  and  land  vertically,  freeing  the  user 
from  operational  constraints  typically  experienced  with 
fixed  wing  aircraft  due  to  the  reliance  on  airfields  or 
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large  clearings.  The  vertical  takeoff  and  landing  (VTOL) 
capability  also  eliminates  any  need  for  and  launching  or 
recovery  equipment. 

Jb .  Speed 

Although  the  quadrotor  is  not  as  fast  as  a  fixed- 
wing  aircraft,  it  does  have  a  speed  advantage  over  many 
other  vehicles  such  as  blimps  and  ground  vehicles.  The 
ability  to  take  a  direct  route  to  an  area  of  interest  also 
gives  it  an  advantage  over  ground  vehicles  that  may  be 
forced  to  avoid  obstacles  such  as  plant  growth. 

c.  Size 

Current  technology  levels  in  batteries  and 
electric  drive  motors  allow  us  to  build  quadrotors  that  are 
small  enough  to  fit  through  doorways  and  maneuver  indoors 
(although  collecting  sensor  data  indoors  can  be 
problematic)  due  to  the  loss  of  GPS  data.  This  ability  to 
fit  through  doorways  gives  the  user  more  flexibility  when 
deploying  the  vehicle  in  an  urban  environment.  While 
employed  in  a  tactical  scenario,  the  size  of  the  quadrotor 
also  means  that  it  has  more  survivability  and  a  smaller 
chance  of  detection  by  enemy  forces. 

d.  Mechanical  Simplicity 

Standard  helicopters  use  a  tail  rotor  to  balance 
the  torque  created  by  the  single  rotor  head  and  use  a 
mechanically  complex  rotor  hub  to  change  the  pitch  of  the 
blades  on  the  main  and  tail  rotors.  On  the  other  hand, 
quadrotors  use  counter-rotating  propellers  to  eliminate  the 
torque  produced  by  the  blades,  negating  the  need  for  a  tail 
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rotor.  Maneuvering  a  quadrotor  is  accomplished  by  changing 
motor  speeds;  an  approach  that  is  mechanically  simple 
compared  to  changing  the  pitch  of  the  blades  using  a  rotor 
hub.  Because  the  quadrotor  uses  four  sets  of  blades,  each 
has  a  smaller  diameter  to  an  equivalent  sized  helicopter.  A 
smaller  set  of  blades  possess  much  less  kinetic  energy  for 
the  same  lifting  force,  reducing  the  potential  for  damage 
to  occur  in  the  event  of  a  collision. 

2 .  Quadrotor  Disadvantages 

Although  quadrotors  have  many  advantages,  they  also 
have  a  few  drawbacks.  The  use  of  continual  battery  power 
during  flight  limits  mission  durations.  The  nature  of  some 
missions  such  as  signals  intelligence  may  allow  the  vehicle 
to  land  while  loitering,  battery  life  still  limits  range  in 
this  case.  Quadrotors  are  also  sensitive  to  disturbances 
such  as  wind  gusts  or  rotor  wash  from  nearby  aerial 
vehicles.  Payload  restrictions  also  limit  the  size  and 
number  of  sensors  the  vehicle  is  able  to  carry. 

C .  RELATED  WORK 

1 .  General 

Many  universities  are  using  quadrotors  in  their 
curricula  and  conducting  research  for  control  and 
trajectory  generation  for  these  vehicles.  Individual 
companies  have  also  started  to  develop  their  own  quadrotor 
systems,  most  aimed  at  the  commercial  market. 
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2. 


University  of  Pennsylvania 


When  you  type  in  "quadrotor"  for  a  Google  search,  the 
first  website  you  find  is  a  Wikipedia  article,  the  second 
is  a  Youtube  video  showing  the  University  of  Pennsylvania' s 
(UPenn)  quadrotors  making  very  aggressive  maneuvers  in 
their  indoor  lab.  The  UPenn  uses  a  quadrotor  developed  by 
Ascending  Technologies  to  verify  their  trajectory 
generation  and  control  algorithms.  The  UPenn  quadrotor 
system  uses  an  external  localization  system  (VICON)  which 
consists  of  20  cameras  and  the  onboard  inertial  measurement 
unit  (IMU)  for  state  estimation  [8]  .  The  video  shows  that 
they  were  able  to  perform  up  to  three  flips,  ascend  and 
descend  through  a  narrow  window  orientated  horizontally, 
and  perch  on  an  inverted  surface  [9] .  The  trajectories  were 
made  possible  by  using  different  controllers  at  different 
stages  of  the  maneuver. 


Figure  1.  University  of  Pennsylvania  Quadrotor  after 

Descending  through  an  Open  Window 
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Figure  2 .  University  of  Pennsylvania  Quadrotor  Perched  on 

an  Inverted  Surface 

UPenn  has  also  been  experimenting  with  micro  UAVs; 
unmanned  vehicles  that  are  between  0.1-0. 5  meters  in  size 
and  0.1-0. 5  kilograms  [10] .  UPenn  uses  these  micro  UAVs  for 
research  into  formation  flying  [4]  and  building  structures 
using  the  vehicles.  UPenn  uses  a  leader-follower  approach 
to  formation  following  where  the  leader  may  be  a  real  or 
virtual  vehicle  [11]  .  In  order  to  simulate  a  construction 
task,  multiple  micro  UAVs  were  used  to  cooperatively 
transport  relatively  large  blocks  of  wood  along  a  three- 
dimensional  trajectory  [12]  .  The  micro  UAVs  used  in  the 
previous  setups  also  use  the  Vicon  camera  setup  and  an  IMU 
to  determine  the  states  of  the  micro  UAVs  [11] . 

3 .  Standf ord  University 

Compared  with  UPenn,  Stanford  has  taken  a  different 
approach  to  their  quadrotor  program.  Stanford  has  developed 
their  own  vehicle  named  Stanford  Testbed  of  Autonomous 
Rotorcraft  for  Multi-Agent  Control  (STARMAC) .  STARMAC  uses 

a  400MHz  processor  with  global  positioning  system  (GPS) ,  an 
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IMU,  and  sonar  altitude  sensors.  Stanford's  research  has 
focused  on  outdoor  applications,  specifically  studying  the 
effects  on  vehicle  flight  due  to  vehicular  velocity,  angle 
of  attack,  and  airframe  design  [13] . 

4 .  Draganf ly  Innovations  Quadrotor 

Draganfly  Innovations,  Inc.  is  a  based  out  of 
Saskatoon,  Canada  and  manufactures  different  rotorcraft, 
each  with  a  different  rotor  setup.  Draganflyers  are 
equipped  with  a  full  suite  of  sensors  including  gyroscopes, 
accelerometers,  and  barometric  pressure  sensors  for  state 
estimation.  Their  standard  quadrotor  has  the  capability  to 
carry  a  0.25  kg  payload  and  interface  with  three  different 
cameras  [14] .  Law  enforcement  agencies  have  found  a  use  for 
the  Draganf Iyer  for  crime  scene  investigations.  Draganfly 
Innovations  also  has  customers  in  the  industrial  circle  as 
well  as  photographers  and  universities. 


Figure  3.  Draganf Iyer  X-4 
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Researchers  at  the  Massachusetts  Institute  of 
Technology  (MIT)  use  the  Draganflyer  to  investigate  swarm 
techniques  that  will  allow  continuous  operations  using 
multiple  vehicles  in  a  dynamic  environment.  MIT's  mission 
leads  them  to  focus  on  health  monitoring  systems,  mission 
task  coordination,  and  a  user  interface  that  supports 
continuous  daily  activities  [15]. 

5 .  Parrot  Drone 

A  prime  example  of  an  affordable  commercial  quadrotor 
is  developed  as  a  toy  is  built  by  Parrot,  a  company  better 
known  for  electronics  such  as  car  stereos.  Parrot's 
AR. Drone  uses  a  Wi-Fi  signal  that  links  to  an  iphone,  iPod 
Touch,  iPad,  Android  smartphone,  or  a  PC  using  the  Linux 
operating  system  [16] .  The  AR. Drone  sends  a  video  feed  from 
a  forward-looking  camera  located  on  the  quadrotor  that  can 
be  displayed  on  the  controlling  device.  The  vehicle  uses  an 
IMU  and  an  onboard  sonar  sensor  to  determine  vehicles 
states,  combined  with  a  feature  tracking  algorithm  that 
utilizes  the  downward  looking  camera  for  hovering.  In 
addition  to  normal  flight  mode.  Parrot  has  developed  an 
application  that  allows  pilots  to  link  with  other  AR. Drone 
owners  and  dogfight.  The  application  uses  visual 
recognition  software  to  determine  if  a  "hit"  was  scored  on 
the  opposing  player's  brightly  colored  hull.  Although  the 
AR. Drone  does  have  a  hovering  capability  that  allows  the 
vehicle  to  hold  altitude  and  position,  it  does  not 
incorporate  much  autonomy  in  the  design  and  requires 
constant  inputs  from  a  pilot  during  forward  flight. 
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Figure  4.  Parrot's  AR. Drone  with  the  Outdoor  "Hull" 

D .  MOTIVATIONS 

Unmanned  systems  are  the  leading  edge  of  a  lot  of 
development  in  the  military  and  civilian  sectors  for  many 
reasons.  Unmanned  systems  have  the  ability  to  remove  human 
operators  from  the  aircraft,  thus  removing  them  from 
danger.  Removing  the  human  operator  from  the  cockpit  also 
removes  the  need  for  a  single  pilot  to  endure  long 

missions.  Instead  of  one  pilot  at  the  controls,  a  shift  of 
operators  can  monitor  the  vehicle  and  change  out  personnel 
to  reduce  the  monotony  and  fatigue  that  accompanies  long 

missions.  The  removal  of  the  pilot  also  means  a  reduction 
in  training  and  re-certification  of  pilots  and  operators. 
If  an  unmanned  vehicle  can  land  itself  on  a  surface  ship  at 
night  without  the  need  for  human  interference,  there  is  no 
need  for  the  waste  of  fuel  and  mission  resources  for  a 

human  to  practice  the  procedure. 

With  the  rise  in  use  of  unmanned  systems  and  the 

decrease  in  defense  budgets,  the  next  evolutionary  step  is 
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an  increased  use  of  autonomy.  An  autonomous  system  will 
decrease  the  need  for  operator  interaction  with  the 
vehicle.  This  reduction  in  human  interaction  with  the 
vehicle  means  that  multiple  systems  can  be  operated  by  one 
individual  for  a  swarm  attack,  surveillance  from  multiple 
angles,  or  a  sustained  surveillance  mission  using  shifts  of 
vehicles.  Alternatively,  a  single  operator  could  operate  a 
smaller  number  of  vehicles  and  still  be  free  to  perform 
another  task  in  the  field. 

E .  OBJECTIVE 

The  objective  of  this  thesis  is  increasing  the 
autonomy  of  a  fleet  of  homogenous  unmanned  vehicles, 
specifically  quadrotors  used  in  unison.  First,  a  trajectory 
generator  will  be  designed  for  a  single  vehicle.  Once  the 
single-vehicle  approach  has  been  verified,  the  method  will 
be  modified  for  use  with  multiple  vehicles  to  ensure  de- 
confliction  of  trajectories  to  avoid  collisions. 
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II.  QUANSER  LAB  SETUP 


A.  INTRODUCTION 

The  laboratory  where  all  trials  were  completed  was 
designed  in  an  open  architecture  and  reconf igurable 
fashion.  The  lab  provides  an  indoor  area  where  all 
environmental  factors  can  easily  be  controlled.  All  Quanser 
equipment  can  be  controlled  in  the  lab,  including  the 
Qball-X4  quadrotor  used  for  demonstration  purposes  in  this 
thesis . 

The  ground  station  is  positioned  at  the  edge  of  the 
room  to  allow  for  a  large  area  in  the  center  where  vehicles 
can  be  maneuvered.  The  floor  is  covered  in  a  thin  foam 
material  to  eliminate  optical  reflections  from  the  floor 
that  may  cause  disturbances  with  the  Optitrack  system. 

A  host  model  is  run  on  the  ground  station  computer 
that  collects  localization  data  from  the  Optitrack  infrared 
camera  system  and  joystick  using  Matlab  Simulink.  The  host 
model  is  started  first  and  sends  all  localization  and 
joystick  data  to  the  vehicles  via  an  ad-hoc  wireless 
network . 

The  control  model  is  built  in  Matlab  Simulink,  then 
downloaded  and  compiled  into  an  executable  onboard  the 
embedded  Gumstix  computer.  The  Gumstix  computer  is 
connected  to  the  HiQ  data  acguisition  card  (DAQ)  that 
encompasses  an  IMU  and  an  avionics  input/output  (I/O)  card 
[17]  . 
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B.  QUANSER  REAL-TIME  CONTROL  SOFTWARE 

The  Quanser  Real-Time  Control  (QuaRC)  software  allows 
for  rapid  prototyping  and  testing  of  control  software  on 
the  Qball  and  Qbot .  The  use  of  Simulink  enables  the  user  to 
skip  the  low-level  programming  and  focus  on  controller 
design.  The  QUARC  package  comes  with  a  Simulink  menu  option 
as  well  as  an  additional  blockset  in  the  Simulink  library 
browser  to  interface  with  all  Quanser  products  as  well  as 
the  Optitrack  camera  system. 
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Figure  5.  Quanser  Simulink  Blockset 


Simulink  files  are  run  under  external  mode  to  allow 
for  the  code  to  be  built  and  run  on  a  target  vehicle  and 
allows  it  to  use  more  computing  resources  so  that  it  can  be 
run  at  a  higher  update  rate.  Multiple  models  can  be  run 
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from  the  same  computer,  allowing  a  single  ground  station  to 
operate  multiple  vehicles.  The  design  of  the  QuaRC  software 
allows  the  user  to  modify  certain  aspects  of  the  code  such 
as  changing  gains  while  the  code  is  running.  This  ability 
to  tune  gains  on  the  fly  greatly  reduces  time  spent 
compiling  and  allows  to  tune  controller  gains  in  flight. 

C .  QBALL-X4 

1 .  Introduction 

The  Qball-X4  is  a  guadrotor  helicopter  that  is 
approximately  0.7  meters  in  diameter  with  an  external 
carbon  fiber  protective  cage.  The  guadrotor  is  powered  by 
four  motors  mounted  to  the  cross-body  that  each  turn  a  10- 
inch  fixed-pitch  propeller.  The  vehicle  is  controlled  by  an 
onboard  Gumstix  computer  and  is  powered  by  two  triple  cell 
11.1  volt  lithium  polymer  batteries  (Figure  10)  [17] . 

2 .  Main  Components 

a.  Protective  Cage  and  Frame 

The  external  cage  is  made  from  thin  carbon  fiber 
rods  that  have  been  glued  into  plastic  connectors.  The  cage 
is  assembled  into  a  shape  similar  to  a  truncated 
icosahedron.  The  shape  provides  rigidity  when  the  quadrotor 
is  resting  on  the  ground,  yet  is  weak  enough  that  it  will 
absorb  the  shock  from  an  impact  if  it  were  to  happen.  If 
the  joints  are  glued  properly,  the  carbon  fiber  rods  simply 
pull  out  of  their  sockets  in  the  plastic  connectors  during 
a  crash  and  are  easily  glued  back  in  place  with  a  hot  glue 
gun.  The  cage  is  recessed  at  the  bottom  to  allow  for  a 
stable  landing  and  to  houses  a  sonar  sensor  for  altitude 
measurements . 


13 


Figure  6. 


Qball-X4  Cage  and  Frame 


The  frame  is  an  aluminum  crossbeam  structure  that 
connects  directly  to  the  protective  cage  via  a  small  rubber 
mount  in  order  to  reduce  shock  loading  in  the  event  of  a 
collision  or  hard  landing.  The  frame  serves  as  a  rigid 
mount  for  all  four  motors  as  well  as  the  HiQ  DAQ  and 
batteries . 


Jb.  Gumstix  Embedded  Computer  and  DAQ 

The  HiQ  is  the  data  acquisition  card  that,  along 
with  the  Gumstix  embedded  computer,  is  responsible  for 
control  of  the  vehicle  and  reading  on-board  sensors.  The 
HiQ  runs  a  Linux  operating  system  and  has  several  high 
resolution  sensors  [17]  for  vehicle  control  and  provide  the 
following  interfaces: 

•  10  pulse  width  modulated  (PWM)  outputs  for 
motor  control 

•  3-axis  gyroscope 

•  3-axis  accelerometer 

•  6  analog  inputs 
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•  3-axis  magnetometer 

•  8  channel  RF  receiver  input 

•  4  Maxbotix  sonar  inputs 

•  2  pressure  sensors  (absolute  and  relative 
pressure) 

•  11  reconf igurable  digital  I/O 

•  2  TTL  serial  ports 

•  Serial  GPS  input 


Figure  7  .  HiQ  DAQ 


c .  Sensors 

For  many  reasons,  not  all  listed  sensors  were 

used  in  the  control  model.  The  magnetometer  is  installed  on 

the  HiQ  DAW  and  has  an  accuracy  of  0.5  mGa/LSB.  During 

testing  it  was  determined  that  the  magnetometer  was 

unreliable,  presumably  because  of  magnetic  fields  from 

nearby  electrical  wires  in  the  lab.  Since  the  magnetometer 

was  deemed  unusable,  only  the  gyroscope  and  accelerometer 
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were  used  for  control  of  roll,  pitch,  and  yaw.  The 
accelerometer  has  a  resolution  of  3.33  mg/LSB  and  the 
gyroscope  is  reconf igurable  for  ±75°/s,  ±150°/s,  or  ±300°/s 

with  a  resolution  of  0.125°/s/LSB  at  a  setting  of  ±75°/s 
[17]  . 

When  analyzing  the  height  model,  it  was 
determined  that  the  sonar  gave  a  very  accurate  estimate  of 
the  altitude,  and  was  deemed  the  sensor  of  choice.  The 


sonar  used 

is  a  Maxbotix  XL-Maxsonar 

EZ3 

that 

is  capable 

of 

measuring 

altitudes 

between  20  cm 

and 

765 

cm  with 

1cm 

resolution 

[17]  .  The 

sonar  is  fixed 

to 

the 

bottom  of 

the 

protective 

cage,  so 

a  correction  must 

be 

made  for 

the 

height  difference  between  the  location 

of  the  sensor 

and 

the  center  of  gravity  of  the  vehicle  where  the  body-fixed 
coordinated  frame  is  located.  Another  specification  to  note 
here  is  that  the  sonar  sensor  measures  a  relative  height 
above  ground  and  will  give  an  incorrect  reading  if  another 
object  is  below  the  sensor,  such  as  another  vehicle  or 
other  obstacle. 
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Lastly,  since  GPS  is  unavailable  or  unreliable 
indoors,  no  onboard  sensors  for  horizontal  position  were 
used.  Instead,  an  external  infrared  camera  tracking  system 
called  Optitrack  was  used  and  position  data  was  few  to  the 
vehicle  over  an  ad-hoc  wireless  connection  from  the  ground 
station.  The  Optitrack  system  uses  infrared  cameras  and 
light  emitting  diodes  (LEDs)  to  track  the  position  of  one 
or  multiple  reflectors  located  on  the  cage  of  the  vehicle. 
The  Optitrack  system  will  be  covered  in  depth  in  a  follow- 
on  section. 


Figure  9.  Reflector  for  the  Optitrack  System 

d.  Motors  and  Propellers 

The  quadrotor  is  powered  by  four  E-Flite  Park  400 
motors  [18].  The  motors  each  turn  a  10x4.7  propeller.  The 
propellers  are  designed  so  that  the  front  and  back 
propellers  spin  in  a  clockwise  direction  and  the  left  and 
right  propellers  spin  counterclockwise.  The  opposing 
rotational  direction  balances  the  yawing  moment  created  by 
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the  spinning  propellers  and  gives  the  quadrotor  control 
over  yaw  angle  with  and  decoupling  it  from  other  state 
variables . 

Each  motor  is  controlled  by  an  electronic  speed 
controller  (ESC,)  shown  in  Figure  10.  Each  ESC  is  connected 
to  the  HiQ  via  the  PWM  portion  of  the  I/O  card  on  the  HiQ. 
The  HiQ  sends  a  command  to  the  ESC  between  1ms  and  2ms .  A 
command  of  1ms  corresponds  to  minimum  throttle  and  a 
command  of  2ms  corresponds  to  maximum  throttle. 


Figure  10.  ESCs  and  Batteries 


3 .  Communication 

The  Qball-X4  and  Qbot  are  designed  to  be  controlled 
via  a  peer-to-peer  transmission  control  protocol/Internet 
protocol  (TCP/IP)  wireless  Internet  connection,  but  other 
protocols  may  be  used.  Because  this  thesis  involves 
communication  between  multiple  vehicles,  the  user  datagram 
protocol  (UDP)  was  selected.  A  UDP  connection  allows  data 
to  be  sent  between  systems  without  first  establishing  a 
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connection  between  the  two  systems  [1] .  The  ability  to  send 
and  receive  data  without  first  establishing  a  connection 
gives  a  lot  of  flexibility  when  testing  models.  In  the  case 
of  this  thesis,  using  a  UDP  connection  allowed  the  testing 
of  one  vehicle  at  a  time  followed  by  a  test  of  multiple 
vehicles  without  altering  reconfiguring  communication  setup 
in  the  models. 


Figure  11.  Connection  Diagram 

The  wireless  Internet  connection  on  the  ground  station 
is  established  by  utilizing  a  USB  Cisco  Linksys  G  type 
wireless  adapter.  After  a  Quanser  vehicle  is  booted  up,  it 
automatically  creates  a  network  named  "GSAH."  Once  the 
vehicle  has  created  the  GSAH  network,  the  user  connects  via 
standard  Windows  procedures.  Next,  the  Internet  protocol 
(IP)  address  is  typically  pinged  to  verify  a  connection  to 
the  vehicle  before  attempting  to  load  a  control  model.  When 
the  control  model  is  run,  the  vehicle  starts  sending  data 
if  it  is  setup  to  do  so.  When  multiple  vehicles  are  used, 
each  is  setup  to  transmit  on  a  different  port  number  and 
can  transmit  any  data  available  such  as  trajectory  or 
current  position.  The  UDP  connection  allows  the  quadrotor 
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to  transmit  this  data,  and  any  other  system  connected  to 
the  GSAH  network  listening  on  the  correct  port  will  receive 
the  data.  This  configuration  allows  a  large  swarm  of 
vehicles  to  communicate  without  unnecessarily  complex 
communication  networks. 

D.  OPTITRACK  MOTION  CAPTURE  SYSTEM 

1 .  Introduction 

The  Optitrack  camera  tracking  system  built  by  Natural 
Point,  uses  infrared  LEDs  and  cameras  to  track  the  position 
of  a  set  of  reflectors  attached  to  the  quadrotor.  The 
system  is  run  by  the  ground  station  computer,  where  the 
vehicle  coordinates  are  calculated,  then  sent  to  the 
vehicle.  The  Optitrack  system  is  necessary  because  GPS 
position  data  is  unavailable  in  the  indoor  environment. 

2 .  Camera  Setup 

The  Optitrack  system  has  the  ability  to  use  up  to  24 
cameras  to  capture  up  to  32  rigid  bodies  at  100  Hz  with  an 
accuracy  of  1  cm  [20] .  The  laboratory  setup  used  11  V100:R2 
cameras  that  each  have  a  field  of  view  of  46  degrees.  Each 
camera  has  a  resolution  of  640x480  pixels  at  a  frame  rate 
of  100  frames  per  second.  The  cameras  were  mounted 
approximately  ten  feet  from  the  laboratory  floor  at  the 
edges  of  the  room  to  give  them  the  largest  capture  volume 
possible.  The  capture  volume  where  the  system  can  track  a 
reflector  is  approximately  10  feet  tall  and  12  by  18  feet 
in  width. 
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Figure  12.  V100:R2  Infrared  Camera 

3.  Tracking  Tools  Software 

The  Tracking  Tools  software  is  used  with  the  Optitrack 

system  and  manages  the  configuration  of  cameras  and 

distinct  reflector  patterns  used  on  each  vehicle.  To 

calibrate  the  system,  a  wand  (included  with  the  Optitrack 

package)  is  used  to  triangulate  the  position  of  each  of  the 

cameras.  After  opening  the  Tracking  Tools  software,  the 

option  to  create  a  new  calibration  file  is  selected.  A  tool 

within  the  software  allows  the  user  to  mask  any  reflections 

caused  by  objects  in  the  field  of  view  of  the  cameras  so 

that  they  don't  interfere  with  tracking  the  reflectors  on 

the  vehicles.  Next,  the  "Start  Wanding"  button  is  selected 

and  the  user  moves  the  wand  through  the  capture  volume 

until  the  Tracking  Tools  software  indicates  that  each 

camera  has  reached  the  desired  accuracy.  Figure  13  shows 

the  view  from  one  camera  during  the  wanding  process,  note 

the  solid  red  squares  are  masks  for  reflective  objects  in 

the  capture  volume.  Once  the  wanding  is  complete,  the  user 
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selects  the  "Apply  Result"  button  and  the  software 
calculates  the  relative  position  and  orientation  of  each 
camera.  Once  this  has  finished,  the  user  places  a  ground 
plane  tool  in  the  center  of  the  capture  volume  on  a  level 
surface  and  the  software  sets  this  as  the  ground  plane  and 
numeric  origin  where  all  measurements  will  relate  to.  Once 
this  is  complete,  the  calibration  is  saved  so  that  it  may 
be  referred  to  later  when  setting  up  the  host  ground 
station  model.  If  multiple  vehicles  will  be  used,  a  file 
must  be  created  that  allows  the  software  to  distinguish  the 
vehicles  from  one  another.  To  accomplish  this,  the  vehicles 
are  placed  in  the  capture  volume,  the  reflectors  are 
selected,  and  the  "create  trackables"  button  is  selected 
through  the  right  click  menu.  This  trackable  file  is  now 
saved  and  referred  to  later  in  the  host  ground  station 
model.  After  the  calibration  is  complete,  the  software 
allows  you  to  see  the  volume  where  the  cameras  can 
determine  the  position  of  reflective  markers.  Figure  14 
displays  the  capture  volume  used  in  this  lab.  The  blue 
boxes  show  the  capture  volume,  the  blue  pyramid  shapes  are 
the  cameras  and  the  black  grid  is  the  ground  plane. 
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Figure  13. 
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Figure  14.  Capture  Volume 


4 .  Connectivity 

All  Optitrack  cameras  are  controlled  by  the  ground 
station  computer.  All  cameras  are  connected  to  an  Optitrack 
hub  via  USB  2 . 0  cables  according  to  the  OptiHub  Setup  guide 
shown  in  Figure  15.  All  hubs  are  interconnected  via  a 
synchronization  cable  and  each  hub  is  connected  to  the 
ground  station  computer  via  USB  2.0  cables.  Up  to  five 
meters  of  extension  cables  may  be  used  to  connect  the  hubs 
to  the  ground  station.  Due  to  the  size  of  the  lab  one 
extension  cable  was  used  for  two  of  the  hubs. 
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OptiHub  Setup  with  18  vlOO  R2  Cameras 
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OptiHub  Setup  Guide 


III.  MODELING  AND  CONTROL  OF  THE  QUADROTOR 


A.  INTRODUCTION 


Before  a  trajectory  generator 

can 

be  built. 

one 

must 

first  understand 

the 

dynamics 

of 

the 

vehicle . 

From 

the 

understanding  of 

the 

dynamics 

of 

the 

vehicle. 

one 

can 

determine  the  aspects  of  a  feasible  trajectory  as  well  as 
what  controls  need  to  be  calculated  to  follow  the 
trajectory.  The  dynamics  of  the  quadrotor  will  be 

represented  by  a  state  space  equation.  The  state  space 
equation  uses  a  set  of  matrices  to  set  up  a  series  of 
first-order  differential  equations  of  the  vehicle  states. 
Some  flexibility  exists  in  defining  the  matrix  representing 
the  vehicle  state;  called  the  state  variable.  The  state 

variable  can  be  selected  through  appropriate  assumptions  or 
approximations  such  as  linearizing  around  a  stable 
condition . 

1.  Assumptions 

In  order  to  simplify  the  complexity  of  the  model,  the 
following  assumptions  were  made 

•  The  Earth  is  flat  and  not  rotating. 

•  The  acceleration  due  to  gravity  is  a  constant 

9.81  m/s . 

•  The  quadrotor  is  symmetric  about  the  pitch  and 

roll  axes  and  the  moment  of  inertia  about  the  x 
and  y  axes  are  equal. 

•  The  quadrotor  frame  is  a  rigid  body  and  does  not 
flex . 
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Pitch  and  roll  angles  are  small. 


•  Vehicle  speeds  are  slow  enough  that  drag  forces 
are  negligible. 

2 .  Coordinate  Frames 

In  this  paper,  two  coordinate  frames  are  used;  the 
body-fixed  and  local  tangent  plane.  The  body-fixed  frame  is 
fixed  to  the  center  of  mass  of  the  quadrotor  and  rotates 
with  the  vehicle.  A  Cartesian  coordinate  system  is  used 
with  the  x-direction  pointing  toward  the  front  of  the 
vehicle,  the  y-direction  points  to  the  left  side  of  the 
vehicle,  and  the  z-direction  is  upward.  To  determine  angle 
directions,  the  right-hand-rule  is  used.  For  example,  a 
positive  roll  angle  will  rotate  the  vehicle  about  the  x- 
axis  in  a  direction  shown  in  Figure  16.  An  orange  sticker 
is  placed  on  the  vehicle  frame  in  the  negative  x-direction 
to  mark  the  tail  section  and  avoid  confusion  of  vehicle 
orientation  during  flight.  The  local  tangent  plane  (LTP)  is 
also  a  Cartesian  coordinate  system  that  rotates  with  the 
earth.  The  LTP  approximates  the  earth  as  a  flat  object  and 
assumes  that  the  earth  is  not  moving.  These  approximations 
can  be  done  because  most  quadrotor  flights  are  very  short 
in  duration;  hence  they  cannot  travel  far  enough  to 
experience  neither  the  curvature  of  the  planet  nor  the 
effects  of  the  spinning  earth.  The  LTP  uses  the  x  and  y- 
directions  in  the  horizontal  plane  and  the  positive  z- 
direction  is  upward. 
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Figure  16.  Quadrotor  Body-Fixed  Coordinate  Frame 

B .  DYNAMICS 

1 .  Actuator  Dynamics 

The  quadrotor  has  four  separate  motor/propellers  that 
can  be  independently  controlled.  Through  control  of  these 
four  motors,  the  second  derivative  of  roll,  pitch,  yaw,  and 
height  in  the  body-fixed  frame  can  be  directly  controlled. 
Through  the  control  of  these  second  derivatives,  we  can 
obtain  control  of  position  and  yaw  angle  in  the  LTP  frame. 

The  generated  thrust  from  each  propeller  is  modeled  by 
the  following  first  order  system 
F  =  K-f-u 

s+co 

where  F  is  the  generated  force,  K  is  a  positive  gain,  co  is 
the  actuator  bandwidth,  and  u  is  the  PWM  input  to  the 
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motor.  To  simplify  the  model,  a  state  variable,  v  will  be 
used  to  represent  the  motor  dynamics  as 


V  =  -7 -U 

S+CO 


For  clarity,  the  motors  on  the  quadrotor  have  been 
numbered  in  according  to  their  location  and  direction  of 
rotation.  For  the  duration  of  this  thesis,  a  number 
subscript  will  be  used  to  denote  each  motor  in  accordance 
with  Figure  17  [17] . 


Figure  17.  Motor  Direction  And  Numbering 

2 .  Pitch  and  Roll  Model 
a.  Introduction 

A  pitch  moment  is  created  by  increasing  the  lift 
created  by  motor  number  one  and  decreasing  the  lift  created 
by  motor  number  two  by  an  equal  amount.  A  roll  moment  can 
be  created  in  a  similar  fashion  by  varying  the  lift  force 
generated  by  motors  three  and  four.  The  rotations  caused  by 
the  pitch  and  roll  moments  will  be  about  the  center  of  mass 
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of  the  vehicle  and  rotations  about  each  axis  are  assumed  to 
be  decoupled. 

Jb .  Model 

For  the  roll  and  pitch  model,  we  first  need  to 
establish  some  constants  used  in  the  model.  A  list  of 
values  determined  by  Quanser  for  the  Qball-X4  can  be  found 
in  Table  1.  J  is  used  as  the  mass  moment  of  inertia  of  the 
quadrotor  about  the  x  and  y  axes.  L  is  the  distance  from 
the  motor  to  the  center  of  gravity  of  the  vehicle. 
AFp=Fl-F2  is  the  difference  in  lift  force  between  the  front 
and  rear  motors  and  AFr=F3-F4  is  the  difference  in  lift 
force  between  the  left  and  right  motors.  The  roll  and  pitch 
rates  denoted  <j)  and  9  respectively  can  be  modeled  as 
J(j)  =  A FpL 
J9  =  A FrL 


Parameter 

Value 

K 

120  N 

CO 

15  rad/s 

J 

0.03  kg  m2 

M 

1.4  kg 

Ky 

4  N  m 

Jyaw 

0.04  kg  m2 

L 

0.2  m 

Table  1.  System  Parameters  [From  17] 
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If  we  use  Au=ul-u2  and  A  u  =  u3-u4  in  conjunction 
with  previously  developed  formulas,  we  may  put  together  the 
following  state  space  eguations  for  the  pitch  and  roll 
model . 
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c.  Control 
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The  controller  developed  by  Quanser  uses  the  same 
setup  for  the  pitch  and  roll  controllers  because  it  is 
assumed  that  the  vehicle  is  symmetric  about  the  two  axes. 
The  controller  developed  by  Quanser  is  a  linear  quadratic 
regulator  (LQR)  that  includes  an  integrator  in  the  state 
variable.  In  order  to  include  the  integrator  into  the 
control  law,  equations  (1)  and  (2)  are  appended  as  follows. 
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(3) 


(4) 


The  LQR  controller  is  designed  assuming  a  state 
space  model  of  the  form  x  =  Ax  +  Bm  where  x  is  the  state 
variable,  A  and  B  are  matrices  determined  by  system 


dynamics,  and  u  is  the  input  to  the  system.  The  control  law 
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is  a  feedback  control  of  the  form  u=-kx.  The  LQR  controller 
uses  a  set  of  weighting  matrices  Q  and  R  that  are  specified 
by  the  designer  in  order  to  minimize  a  set  of  costs  [27]  . 
These  Q  and  R  matrices  in  conjunction  with  the  A  and  B 
matrices  that  describe  the  system,  a  simple  Matlab  command 
can  be  used  to  determine  the  gain,  k  to  control  the  system. 

The  Q  and  R  matrices  developed  by  Quanser  to  be 
used  for  the  pitch  and  roll  controllers  on  the  Qball-X4 
are : 

100  0  0  0 

0  0  0  0 

0  0  22000  0 

0  0  0  10 

R  =  30000 

The  feedback  gain,  k  that  results  from  these 
inputs  results  in  poles  at  -19.827,  -4 . 083+4 . 275i,  -4.083- 

4.275i,  and  -0.316.  In  order  to  ensure  that  the  quadrotor 
stays  in  the  linear  and  stable  region,  limits  have  been 
placed  at  .2  radians  for  both  pitch  and  roll  angles. 

3 .  Yaw  Model 

a .  Model 

The  yaw  model  for  a  quadrotor  is  very  simple  in 
comparison  to  the  other  axes.  The  torque  generated  by  each 
motor,  thus  the  torque  felt  by  the  quadrotor  by  the  air  on 
the  propellers  is  said  to  be  proportional  to  the  PWM  input 
to  the  motors.  If  i  is  the  torque  generated  by  the  motor  u 
is  the  PWM  input  to  the  motor,  and  Ky  is  a  positive  gain 
taken  from  Table  1,  the  following  relationship  is  true: 
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in  the  yaw  axis 


Rotation  in  the  yaw  axis 
difference  in  torques  from  the  motors 
directions.  If  tJj  is  the  yaw  angle  and 
yaw  axis  can  be  modeled  as 


is  driven  by  a 
rotating  in  opposite 
Ar  =  Tj  +  r2  -t3  -r4 ,  the 


JyWy=^ 

Written  in  state  space  form  as: 
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Jb.  Control 


The  yaw  controller  for  the  Qball  was  designed  in 
a  similar  fashion  as  the  pitch  and  roll  controllers.  The  Q 
and  R  matrices  used  are: 


Q. 


yaw 


1  0 
0  .1 


f?  =  1000 


(9) 

(10) 


The  feedback  gain,  k  that  results  from  these 
inputs  results  in  poles  at  -1 . 3532+1 . 1537i  and  -1.3532- 
1 . 1537i . 


4 .  Position  Model 
a .  Model 

Motion  in  the  horizontal  plane,  referred  to  here 
as  the  X-Y  plane  is  due  to  thrust  from  the  propellers  when 
the  vehicle  has  a  non-zero  pitch  or  roll  angle.  When  the 
propeller  thrust  is  not  aligned  with  the  vertical 
direction,  a  component  of  the  thrust  causes  acceleration  in 
the  horizontal  plane.  The  relationship  between  acceleration 
in  the  x  and  y  directions,  the  average  force  from  each 
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motor,  mass,  and  the  pitch  and  roll  angles,  denoted  x ,  y  , 
F,  M,  qp,  and  □  respectively  is 

Mx  =  4F  sin((j>) 

My  =  -4Fsm(6)  ^12) 

If  the  pitch  and  roll  angles  are  close  to  zero, 
the  following  linearization  can  be  made 

"o  i  o  oYxl  To" 

o  o  o  x  o 

+  b 

0  0  -CD  0  V  CD 

10  0  |_0_ 

"o  i  o  oiiyi  ro 

0  0  0  y  +  0 

0  0  -CD  0  v  co 

io  o  ol,J  [o 

Jb.  Control 

An  LQR  controller  was  developed  for  the  X-Y 
position  model  in  the  same  manner  as  the  pitch  and  roll 
controllers.  The  matrices  used  for  the  construction  of  the 
controller  are 

"5  0  0  0 

0  2  0  0 

Q  = 

0  0  0  0 
0  0  0  .1 

i?  =  50  (16) 

The  feedback  gain,  k  that  results  from  these 
inputs  results  in  poles  at  -6.712,  -1 . 61+0 . 7 92i ,  -1.61- 

0.792i,  and  -0.142. 


(13) 

u 

(14) 
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5 .  Height  Model 


a .  Model 

Motion  in  the  vertical  direction  is  caused  by  the 
vertical  component  of  the  thrust  from  the  propellers.  If 
the  thrust  generated  by  each  propeller  is  F,  g  is  the 
acceleration  due  to  gravity,  and  z  is  the  vertical 
acceleration,  then 
Mz  =  4 F  cos {</>)  cos {6)  -  Mg 

The  vertical  channel  can  be  modeled  by  the 
following  state  space  formula 

z  0  1  0  0  z  0  0 

z  _  0  0  ^  0  z  0  g 

v  0  0  -co  0  v  co  0 

ij  |_i  0  o  ol,J  |_oJ  Lo 

b.  Control 

Unlike  the  other  controllers,  the  vertical 
channel  is  controlled  via  a  proportional  integral 
derivative  (PID)  controller.  A  PID  controller  uses  three 
gains  and  information  about  the  state,  its  derivative  and 
the  integral  of  the  state.  The  gains  used  on  for  the  height 
controller  for  the  Qball-X4  are  given  in  Table  2. 


Gain 

Designation 

Value 

Proportional 

KP 

0.00621 

Integral 

Ki 

0.0015 

Derivative 

Kd 

0.0078 

Table  2.  Height  Controller  Gains 
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C.  MODES  OF  CONTROL 

The  model  developed  by  Quanser  allows  the  user  to 
easily  select  closed  loop  control  for  altitude,  yaw  angle, 
and  horizontal  position.  Also,  if  the  user  wants  to  take 
manual  control,  the  model  can  be  configured  on  the  fly  to 
take  commands  from  a  joystick  connected  to  the  ground 
station  computer.  The  commands  from  the  joystick  are  fed 
through  the  host  model  along  with  the  vehicle  position  from 
the  Optitrack  system  over  the  wireless  connection. 
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IV.  DIRECT  METHOD  BASED  TRAJECTORY  GENERATION 


A.  INTRODUCTION 

In  order  to  achieve  full  autonomy,  we  need  to  develop 
a  optimal  trajectory  generator  that  can  spatially  de¬ 
conflict  paths  as  well  as  respect  vehicle  constraints.  The 
routine  must  be  capable  of  updating  the  path  several  times 
throughout  the  flight  in  order  to  account  for  any 
discrepancies  in  the  model  as  well  as  disturbances  in  the 
process.  Most  of  the  known  optimization  software  packages 
such  as  OTIS,  SOCS,  DIRCOL,  or  DIDO,  based  on  collocation, 
direct  transcription  and  pseudo  spectral  methods  [21] -[14], 
require  extensive  computational  power  as  they  involve  many 
varied  parameters  and  therefore  may  not  have  the  ability  to 
be  used  for  on-line  computation  of  agile  short-term 
maneuvers . 

The  method  proposed  to  use  by  this  thesis  is  the 
direct  method  of  calculus  of  variations  exploiting  the 
inverse  dynamics  of  a  vehicle  in  a  virtual  domain  (IDVD) 
[27] .  This  method  is  capable  of  fast  prototyping  of  optimal 
trajectories  for  multiple  vehicles.  It  allows  respecting 
vehicle  constraints  and  assures  collision-free 
trajectories.  By  design  IDVD  method  involves  very  few 
varied  parameters  and  significantly  reduced  computational 
power  [27] .  This  method  is  also  useful  in  that  it  is 
relatively  simple  to  modify  and  use,  giving  the  user  the 
ability  to  modify  the  code  for  a  specific  scenario  or 
change  the  number  of  vehicles  operating  together. 

The  trajectory  generator  gives  a  set  of  x,  y,  and  z 
coordinates  and  corresponding  time  derivatives.  These 
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coordinates  can  be  fed  directly  to  the  quadrotor,  or  the 
vehicle's  inverse  dynamics  can  be  used  to  compute  the 
necessary  controls  to  track  the  reference  trajectory. 

B .  REFERENCE  TRAJECTORY 

In  order  to  create  a  reference  trajectory  independent 
of  any  time  derivative  constraints,  a  path  is  created  using 
a  mathematical  function  for  each  Cartesian  coordinate  using 
a  virtual  variable  "i"  as  the  independent  variable.  Since 
this  reference  function  is  in  the  virtual  domain,  we  have 
completely  decoupled  space  and  time.  Later,  we  will 
introduce  a  variable  speed  factor  to  map  the  function  from 
the  virtual  domain  to  the  time  domain  [25] . 

When  selecting  a  reference  function  (set  of  basis 
functions) ,  we  must  consider  the  general  shape  for  an 
expected  trajectory  as  well  as  the  number  of  boundary 
conditions  that  must  be  satisfied.  In  the  case  of  a 
quadrotor,  a  simple  polynomial  will  give  the  desired  shape 
for  the  reference  function,  but  other  shapes  such  as  a 
sinusoid  may  be  selected  based  on  the  operating 
environment.  We  represent  the  x,  y,  and  z  coordinates  as 
analytically  defined  Nth-order  polynomials  of  the  form 


cs 

II 
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k= 0 

(18) 

N 

y=TjavJk 

(19) 

k= 0 

N 

Z  =  J la*Tk 

(20) 

k= 0 

The  order  of  the  reference 

function  polynomial  N  is 

determined  by  the  number  of  boundary  conditions  that  must 
be  satisfied,  but  can  be  increased  to  add  to  the  degrees  of 
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freedom.  If  do  is  the  highest-order  spatial  derivative  of 
the  set  of  initial  conditions  and  df  is  the  highest  order 
derivative  of  the  final  conditions,  then  the  order  of 
polynomial  used,  is  N  =  d0+df  + 1.  For  example,  if  we  desired 
to  specify  third  order  derivatives  at  the  initial  and  final 
points,  then  d0=  3,  df=  3,  and  7V  =  3  +  3  +  l  =  7. 

To  construct  the  reference  trajectory,  we  must  ensure 
that  all  initial  and  final  conditions  are  satisfied.  In 
order  to  ensure  that  the  polynomial  satisfies  the  velocity 
and  acceleration  at  the  endpoints  of  the  maneuver,  we  must 

dx  dy  dz 

analytically  compute  —  —  —  and  higher  order 

dr  dr  dz 

r  r  r 

derivatives.  Since  we  will  be  computing  coefficients  of 
higher  order  derivatives  later,  it  is  convenient  to  rewrite 
equations  (18) -(20)  and  their  derivatives  as 
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(30) 
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For  this  thesis,  it  was  important  ensure  the  vehicle 


experienced 

a  smooth  transition  at 

the 

beginning 

and 

endpoints  of 

the  trajectory.  It  is  also 

necessary 

to  allow 

for  another 

set  of  varied  parameters 

to 

allow 

for 

the 

algorithm  to 

have  flexibility  over 

the 

shape 

of 

the 

trajectory.  Since  the  initial  and  final  position,  velocity, 
and  accelerations  are  already  specified,  the  third  order 
derivative  called  the  jerk  will  be  used  as  the  varied 
parameter.  Now  we  have  four  initial  and  four  final 
conditions  to  be  satisfied,  calling  for  a  7th  order 
polynomial  of  the  form  expressed  in  equations  (21) -(32) .  If 
we  select  the  final  i  as  a  variable  parameter  and  combine 
equations  (21) -(32),  we  can  setup  a  matrix  equation  to 
solve  for  all  polynomial  coefficients  ax0  through  axl  . 
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Matrix  equations  for  the  two  other  coordinates  are 
similar  to  (33)  and  omitted  here. 
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Now  that  we  have  developed  a  method  for  determining 
the  reference  function,  it  is  useful  to  explore  the 
versatility  of  the  function.  Figure  18  shows  the  effects  of 
varying  the  initial  third  order  derivative  and  final  value 
for  tau.  The  Figure  was  developed  using  the  same  algorithm 
inside  the  trajectory  generator  used  for  this  thesis. 


Figure  18.  Illustration  of  Reference  Function 

Flexibility  Using  Two  Varied  Parameters 

C .  SPEED  FACTOR 

As  previously  stated,  in  order  to  decouple  space  and 
time,  a  speed  factor  was  introduced.  The  speed  factor,  X 
links  the  virtual  domain  to  the  time  domain  by  the 
following  mapping  function 
.  dr 

A=—  (34) 

dt 

In  order  to  allow  for  flexibility  in  the  trajectory 
and  simplicity  of  the  model,  a  polynomial  was  selected  to 
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represent  X(t) .  At  this  point  it  is  convenient  to  visit  the 
matter  of  linking  time  derivatives  of  x  (i) and  x(t)  .  The 
boundary  conditions  used  in  calculating  the  coefficients  of 
the  reference  functions  were  derivatives  of  i  whereas  we 
only  know  the  boundary  conditions  as  derivatives  with 
respect  to  time.  In  order  to  relate  these  boundary 
conditions,  we  simply  set  the  boundary  conditions  of  X  at 
unity  so  that  dA  =  dt  and  all  virtual  derivatives  are  equal 
to  corresponding  timer  derivatives.  To  increase  the 
flexibility  of  the  speed  function,  the  2nd  order  derivatives 
at  the  endpoints  will  be  used  as  varied  parameters  and  the 
1st  order  time  derivatives  will  be  set  at  zero,  giving  us 
six  boundary  conditions  to  satisfy  and  requiring  a  5th 
order  polynomial.  If  N=5,  the  polynomial  and  derivatives  of 
lambda  is  represented  as  [26] 

A  (max(l,  k-  2)) ! 
k= o  k ! 

k-\  (max(l, k-2))\ 


— =A'=y< 

d  r  tt 

r!2  7  N 

u  71  k-2 

r  2  —  ^  —  2~iaAkT 

dr  k=2 


(k~  1)! 


:  35 ) 
:36) 


:37) 


If  we  want  to  satisfy  all  boundary  conditions  and 
equations  (34) -(35)  then  we  can  setup  the  following  matrix 
equation  to  solve  for  the  speed  factor  polynomial 
coefficients  where  A0=Aj-=  1  =  A^  =  0  and  A£  and  A"  are  varied 

parameters . 
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D .  INVERSE  DYNAMICS 


To  determine  the  controls  that  need  to  be  fed  to  the 
vehicle,  the  quadrotors  inverse  dynamics  need  to  be 
determined.  The  quadrotor  uses  roll  and  pitch  angles  in 
coordination  with  a  yaw  angle  to  control  position  in  the 
horizontal  plane  and  propeller  thrust  to  control  vertical 
height.  Since  roll,  pitch,  yaw,  and  thrust  are  used  to 
control  the  vehicle,  we  must  develop  a  set  of  methods  to 
calculate  these  values  to  be  fed  to  the  vehicle  in  order  to 
track  the  trajectory.  From  the  geometry  of  the  quadrotors 
dynamics,  we  can  determine  that  the  pitch  and  roll  angles 
and  thrust  are  [27] 


0  -  arctan 


'  it  A 


\g-zJ 
( 


(j)  =  arcsin 


-y 


•  \2 


F  = 


jx2  -v2 -(g-z) 

(z-g) 


[39] 


:4o: 


(41) 

cos(#)cos(0) 

To  compute  the  time  derivative,  the  speed  factor  and 
virtual  derivatives  of  the  reference  function  were  utilized 
in  the  following  manner  (here  £  represents  any  state)  [27] . 
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The  spatial  trajectory  is  given  in  three  axes,  thus 
only  three  independent  control  parameters  need  to  be  varied 
in  order  to  track  the  trajectory  assuming  the  orientation 
of  the  vehicle  is  not  a  concern.  Since  i)j  effectively  only 
controls  the  orientation  of  the  quadrotor,  it  is  kept  at 
zero.  Later,  this  could  be  varied  in  order  to  aid  in  the 
accomplishment  of  mission  objectives  such  as  reconnaissance 
or  tracking  other  vehicles  with  onboard  sensors. 


E .  COST  FUNCTION 

1.  Single  Quadrotor  Trajectories 


Now  that  we  have  created  a  trajectory  that  satisfies 
the  boundary  conditions,  it  is  time  to  optimize  it.  In 
order  to  optimize  the  trajectory,  we  must  develop  a  cost 
function.  For  the  case  of  one  quadrotor,  we  are  concerned 
with  two  factors;  deviation  in  arrival  time  and  respecting 
the  vehicle  constraints.  The  vehicle  constraints  that  we 
are  concerned  about  are  the  pitch  and  roll  angles  that  will 
be  determined  by  accelerations  in  the  horizontal  plane. 
Limits  in  the  vertical  direction  are  not  considered  here  as 
deviations  in  the  vertical  direction  are  usually  quite 
small.  From  these  constraints,  the  cost  function,  J  was 
constructed  as 


(t  -t  r  * 

J  =  c\  jestrel - sLL  +  C JO 

1  v  2  z 


^ threshold 


C310 


'44' 


1  desired 


Where  Ci,  C2,  and  C3  are  constants  to  be  tuned  later, 
tdesired,  tend,  <Pmax,  and  cpthreshoid  are  the  desired  time  entered 
by  the  user,  end  time  of  the  maneuver,  maximum  pitch  angle 
in  the  maneuver,  maximum  pitch  angle  the  controller  will 
allow  respectively.  The  general  shape  of  each  portion  of 
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the  cost  function  is  parabolic  and  minimized  when  tdesjred  =  tend  , 
cp=0,  and  9=0.  Clearly  we  do  not  want  the  pitch  and  roll 
angles  to  be  zero,  otherwise  the  vehicle  will  not  move  in 
the  horizontal  plane.  The  cost  function  (35)  will  allow  for 
some  deviation  of  the  roll  angles  from  the  horizontal 
position,  but  penalizes  them  more  as  they  approach  the 
threshold  values  for  the  controller. 

2.  Multiple  Quadrotor  Trajectories 

In  the  real-world  situation,  each  vehicle  is  supposed 
to  carry  its  own  "see  and  avoid"  hardware,  so  its  computer 
will  only  be  responsible  for  computation  of  its  own 
collision-avoiding  maneuver.  In  this  thesis  quadrotors  were 
not  supposed  to  possess  a  "see"  capability,  so  that  the 
trajectories  for  both  vehicles  were  computer  in  a 
centralized  manner.  It  means  that  if  we  have  M  vehicles, 
the  individual  number  of  varied  parameters  for  the 
optimization  problem  has  to  be  multiplied  by  M.  In  order  to 
mitigate  the  increase  of  varied  parameters  this  thesis 
assumed  a  certain  symmetry  for  multiple  vehicles  which 
allowed  keeping  the  number  of  varied  parameters  at  the  same 
level  as  for  a  single  vehicle.  Specifically,  each  vehicle 
uses  the  same  speed  factor  and  the  third  order  timer 
derivatives  are  in  opposite  directions  at  the  boundary 
points . 


a.  Simplifications 

For  this  thesis,  the  trajectory  generator  was 
created  for  controlling  only  two  vehicles  at  one  time. 
Using  only  two  vehicles  allows  the  testing  to  take  place  in 
the  small  laboratory  that  is  available  while  still  proving 
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that  multiple  vehicles  can  be  controlled  in  this  manner.  To 
simplify  the  problem  and  decrease  the  number  of  varied 
parameters,  the  same  speed  factor  was  used  for  both 
vehicles.  Using  the  same  speed  factor  still  allows  for  the 
de-coupling  of  space  and  time,  yet  decreases  the  need  for 
extra  varied  parameters .  To  further  decrease  the  number  of 
varied  parameters,  a  relationship  was  created  between  the 
initial  and  final  jerk  for  each  vehicle  that  ensured  the 
two  sets  of  trajectories  diverged  from  one  another.  The 
relationship  used  was 


dixA  _  d7’xB 
dt 3  dt 3 

d'yA  _  d'yB 

dt 3  dt 3 


(45) 

(46) 


Now  that  we  have  a  set  of  equations  relating  the 
third  order  boundary  conditions  of  each  vehicle  in  the 
horizontal  plane,  we  only  need  to  vary  the  parameters  for 
one  vehicle,  which  will  decrease  computational  time. 


Finally, 

we 

often 

do 

not  want  the 

tra j  ectory 

of 

the 

vehicles 

to 

take 

them 

underneath  each 

other  because 

of 

interference 

with 

the 

sonar  sensors 

that  are 

used 

to 

determine  altitude.  It  is  also  deemed  unnecessary  to  allow 
for  extra  freedom  of  motion  in  the  vertical  channel,  so  the 
jerk  in  the  vertical  direction  was  not  used  as  a  varied 
parameter  and  consequently  set  to  zero.  The  list  of 
assumptions  used  here  reduces  the  number  of  varied 
parameters  from  18  down  to  seven. 


b. 

Modi fication 

of 

the 

Cost 

Function 

To 

incorporate 

the 

extra 

vehicle. 

the 

cost 

function  was 

modified  to 

include 

the 

extra  set 

of 

Euler 
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angles,  arrival  time,  as  well  as  an  extra  term  for  the 
distance  between  the  vehicles 


j  0 desired  ^ lend  , 

J  ~  C1  V 


-  +  C2(i 


lend 


L lend 


A Tf  +  C,(dn 


■cl 
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where  dmin  is  the 
vehicles  along  the  trajectory 


minimum  distance  between 
and  dthreshoid  i  S  1.2m. 


(47) 

the 


As  seen  the  Euler  angles  penalties  are  identical 
for  both  vehicles.  The  arrival  time  term  for  the  second 
vehicle  is  essentially  the  same  as  for  the  first  vehicle  if 
Ar  =  0 .  Written  slightly  different  it  allows  sequencing  the 
arrival  time  for  multiple  vehicles  in  the  future  research 
if  needed. 


The  distance  portion  of  the  cost  function  (38) 
was  created  so  that  the  minimum  distance  between  the 
vehicles  would  be  driven  to  a  pre-determined  value  that 
ensured  the  vehicles  did  not  collide.  The  value  that  was 
used  in  this  thesis  was  1.2m,  which  corresponds  to  a 
distance  slightly  greater  than  twice  the  radius  to  allow 
for  a  safety  margin. 


c.  Computing  the  Final  Trajectory 

In  order  to  compute  a  trajectory  that  satisfies 
all  constraints,  a  Simulink  model  is  created  that  solves 
equations  (33)  and  (38)  and  creates  the  trajectory  as  a 
function  of  time.  From  this  trajectory,  the  arrival  time, 
all  vehicle  controls,  and  the  relative  distance  between 
vehicles  can  be  calculated  for  every  point  along  the 
trajectory.  From  this  information  calculated  by  the 
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Simulink  model,  we  can  determine  the  value  of  the  cost 
function.  The  fminsearch  command  is  used  inside  Matlab  to 
run  the  Simulink  model  and  determine  a  set  of  varied 
parameters  that  minimizes  the  cost  function,  hence,  an 
optimal  trajectory.  The  Simulink  model  was  set  to 
discretize  the  model  into  200  equally  spaced  fixed-steps. 
Since  the  output  of  the  function  would  not  match  the 
frequency  of  the  controller,  the  Matlab  command  "interpl" 
was  used  to  perform  a  linear  interpolation  between  the 
points  at  the  controller  frequency  of  200  Hz.  The  final  set 


of  varied 

parameters 

used  with  the  minimization 

function 

d3x 
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tra j  ectory 

has 

been 

computed,  either  the  Euler 

angles 

or 

position  commands  can  be  fed  to  the  quadrotor  controllers. 
The  optimization  routine  will  be  continuously  run  durinq 
the  fliqht  to  allow  for  changes  in  the  mission  and  account 
for  disturbances  or  inaccuracies  in  the  model. 
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V.  ILLUSTRATIVE  MISSIONS  AND  SIMULATION  RESULTS 


1 .  INTRODUCTION 

The  trajectory  generator  was  verified  using  two 
representative  scenarios  that  were  specifically  designed  to 
test  for  collision  avoidance.  By  examining  the  outputs  of 
the  trajectory,  we  were  able  to  confirm  that  the  vehicle 
constraints  and  arrival  time  were  satisfied. 

2 .  APPROACH 

Because  of  time  constraints  and  the  short  duration  of 

maneuvers,  the  trajectory  for  each  quadrotor  was  only 

computed  once  on  the  ground  station  computer  and  fed  to  the 

quadrotors  via  the  wireless  connection.  At  the  beginning  of 

each  flight,  a  waypoint  was  fed  to  each  controller  for  20 

seconds  after  take-off  to  allow  for  the  transient  response 

to  settle  out.  Following  the  initial  20  seconds,  a  set  of 

Euler  angles  was  fed  to  the  pitch  and  roll  controller  and 

an  altitude  command  was  fed  to  the  altitude  controller 

followed  by  a  second  waypoint  at  the  endpoint  of  the 

maneuver.  Since  the  Euler  angles  generated  by  the 

trajectory  were  small,  the  results  from  this  method  were 

unreliable.  When  the  model  is  started,  the  Euler  angles  are 

initialized  at  zero  and  then  the  gyroscope  is  used  to 

update  the  angles  during  flight.  With  this  method  of 

initialization,  if  the  initial  orientation  was  not 

perfectly  level,  there  will  always  be  a  steady  state  error 

in  the  estimation  of  the  orientation  of  the  vehicle.  For 

this  reason,  it  was  decided  to  use  the  position  data  from 

the  trajectory  to  control  the  quadrotors.  During  each 

flight,  there  was  some  lag  in  the  position  of  the 
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quadrotor.  After  observing  the  response  of  the  Qball  to 
position  commands  and  examining  the  controller,  it  is 
believed  that  the  native  controller  developed  by  Quanser  is 
not  optimal  and  needs  further  work  to  increase  the  response 
time . 

All  calculations  were  performed  on  a  desktop  PC  with 
an  Intel  Core  i7  2.79  GHz  processor  and  8GB  of  RAM  running 
Matlab  version  7.10.0. 

3 .  SCENARIO  #1 

The  first  scenario  that  was  tested  was  a  situation 
where  each  quadrotor  started  at  one  edge  of  the  room  from 
rest  (zero  velocity  and  acceleration  in  all  directions)  and 
the  vehicles  were  told  to  swap  places,  again  with  zero 
velocity  and  acceleration.  The  arrival  time  for  the 
maneuver  was  requested  to  be  20  seconds.  This  scenario  was 
selected  to  test  the  ability  of  the  trajectory  generator  to 
create  a  path  that  avoided  collisions.  If  a  simple  set  of 
waypoints  were  given  to  the  quadrotors,  they  would  drive 
toward  each  other  and  collide.  Table  3  gives  a  list  of 
initial  and  final  conditions  for  each  vehicle  that  was  fed 
into  the  trajectory  generator. 


Quadrotor  A 

Quadrotor  B 

x0 

-lm 

2m 

ho 

-0.5m 

-0.5m 

zo 

.  6m 

.  6m 

X/ 

2m 

-lm 

37 

-0.5m 

-0.5m 

z/ 

.  6m 

.  6m 

x0 

0 

0 

v0 

0 

0 
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Table  3.  Scenario  #1  Boundary  Conditions 

The  CPU  time  required  to  create  the  trajectory  was 
25.5374  seconds  for  a  single  run.  The  final  values  for  each 
varied  parameter  can  be  found  in  Table  4  and  a  bird-eye 
view  of  the  trajectory  can  be  seen  in  Figure  19. 


Table  4 . 


Varied  Parameters  for  Scenario  #1 


Figure  19.  Scenario  #1  Reference  Trajectory  and  Actual 

Data 


Figure  20. 


Scenario  #1  Parameters 
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The  trajectory  generator  develops  a  feasible  path  for 
this  scenario  that  mimics  the  path  that  a  human  operator 
would  fly.  The  algorithm  performs  well  here  because  the 
trajectory  selected  is  the  shortest  path  that  still  allows 
for  a  1.2m  separation  between  the  two  vehicles.  A  quick 


look  at  Figure  19 

shows 

the 

lack  of 

performance 

of 

the 

native  controller. 

UAVi 

shown 

in  blue 

was  pushed 

of 

its 

trajectory  due  to 

wind 

turbulence 

caused  by 

the 

two 

vehicles.  This  disturbance  seemed  to  reach  a  maximum  at  the 
midpoint  of  the  maneuver  because  it  was  closest  to  the  edge 
of  the  room  where  the  airflow  pattern  changes.  The 
inability  of  the  controller  to  track  commands  is  displayed 
by  the  performance  of  UAV2 .  The  position  of  the  vehicle  is 
always  inside  the  trajectory  path,  illustrating  the  need 
for  a  safety  margin  when  specifying  the  minimum  threshold 
distance  between  the  vehicles. 

4 .  SCENARIO  #2 

The  second  scenario  tested  the  ability  of  the 
algorithm  to  create  a  trajectory  to  cross  the  paths  of  both 
vehicles.  Again,  both  vehicles  are  starting  from  rest  at  a 
distance  of  1.5m  from  each  other  on  one  side  of  the  lab. 

The  final  desired  position  of  each  vehicle  fed  into  the 
trajectory  generator  was  at  the  other  edge  of  the  lab,  but 
the  quadrotors  must  switch  positions  in  one  coordinate 
before  they  reach  the  other  edge  and  the  desired  time  of 
the  maneuver  was  20  seconds.  Again,  this  scenario  tests  the 
collisions  avoidance  aspect  of  the  algorithm  as  well  as  the 
arrival  time  of  the  vehicles. 
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Quadrotor  A 

Quadrotor  B 

x0 

-0.75m 

-0.75m 

Vo 

-1.25m 

0.25m 

zo 

.  6m 

.  6m 

Xf 

1 .75m 

1 .75m 

y.f 

0.25m 

-1 . 25m 

zf 

.  6m 

.  6m 

x0 

0 

0 

Vo 

0 

0 

4 

0 

0 

0 

0 

y.f 

0 

0 

zf 

0 

0 

x0 

0 

0 

Vo 

0 

0 

*0 

0 

0 

h 

0 

0 

h 

0 

0 

zf 

0 

0 

Table  5.  Scenario  #2  Boundary  Conditions 

The  time  required  to  compute  this  trajectory  was 
slightly  higher  than  scenario  #1  at  45.1935  seconds.  The 
varied  parameters  and  trajectory  that  was  calculated  for 
scenario  #2  can  be  seen  in  Table  6  and  Figures  21  and  22. 
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Varied  Parameter 

Value 

Tf 

16.04 

x\ 

0.0883 

y] 

0.5489 

*f 

0.0883 

y] 

-0.0549 

a: 

-0.0440 

a; 

-0.0440 

Table  6.  Varied  Parameters  for  Scenario  #2 


Figure  2 1 . 


Scenario  #2  Reference  Trajectory  and  Actual 

Data 
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Figure  22.  Scenario  #2  Parameters 


In  scenario  #2,  the  algorithm  develops  a  trajectory 
that  is  not  necessarily  intuitive,  but  does  satisfy  all 
requirements.  The  initial  jerk  of  UAV2  allows  the  other 
quadrotor  to  cross  ahead  and  avoid  a  collision  at  the 
geographic  center  of  the  maneuver.  A  look  at  Figure  22 
shows  that  the  minimum  distance  between  the  vehicles  is 
1.2m  at  the  14  second  mark.  Figure  22  shows  the  lack  of 
performance  of  the  controller  that  was  also  seen  in  Figure 
19.  It  is  important  to  note  that  there  was  an  error  in  the 
arrival  time  of  the  trajectory  of  1.476  seconds.  Although 
this  represents  a  small  deviation  in  this  short  maneuver, 
it  may  represent  a  larger  deviation  in  a  maneuver  in  a 
larger  scenario.  The  deviation  in  arrival  time  could  be 
solved  by  giving  the  user  the  ability  to  adjust  the  gains 
in  the  cost  matrix.  If  arrival  time  is  more  important  to 
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the  user,  then  he  can  select  a  larger  gain  for  Ci  and  C2, 
the  gains  for  the  arrival  time  terms.  A  deviation  in 
arrival  time  in  this  instance  is  also  likely  due  to  the 
poor  performance  of  the  position  controller,  and  will  have 
to  be  reevaluated  once  a  more  accurate  controller  is 
developed . 
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VI.  CONCLUSIONS  AND  FUTURE  WORK 


1 .  CONCLUSIONS 

The  following  conclusions  can  be  drawn  from  the 
research  conducted  within  this  thesis: 

•  We  have  developed  an  architecture  that  allows 
the  control  of  multiple  UAVs  from  a  single 
ground  station. 

•  The  inverse  dynamics  in  the  virtual  domain  based 
trajectory  generator  has  proved  to  be  a  good 
candidate  for  a  "see  and  avoid"  capability  for 
future  unmanned  systems. 

•  The  native  controller  developed  by  Quanser  does 
not  exhibit  an  acceptable  behavior  and  needs  to 
be  optimized  for  better  performance  while 
tracking  spatial  curved  trajectories. 

2.  RECCOMENDATIONS  FOR  FUTURE  RESEARCH 

Recommendations  for  future  work  with  the  Qball-X4 
quadrotor  as  a  result  of  this  thesis  are: 

•  Investigate  latency  issues  in  controlling 
multiple  unmanned  systems. 

•  Improve  the  trajectory  tracking  controller  of  the 
Qball-X4  by  either  optimizing  the  native  LQR 
controller  or  developing  a  model  predictive 
controller . 
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•  Incorporate  more  than  two  quadrotors  or  multiple 
heterogeneous  vehicles  to  test  the  scalability  of 
the  system. 

•  Add  sensors  to  give  each  vehicle  the  ability  to 
sense  the  position  of  other  cooperative  vehicles. 

•  Incorporate  compiled  C++  code  to  enable  reactive 
trajectory  updates  onboard  each  vehicle. 

•  Decrease  computational  load  to  allow  more  rapid 
trajectory  updates  to  enable  a  real-time  system. 

•  Reduce  simplifying  symmetry  assumptions  to  result 
in  more  practical  and  intuitive  solutions. 
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APPENDIX 


MATLAB  DOCUMENTATION 


filter  design. m  file 

This  script  is  run  when  the  Qball-X4  model  is 
initialized  in  order  to  calculate  the  coefficients  for 
complimentary  filters. 
t=10; 

s  =  t  f  ( '  s ' )  ; 

Gg  =  tA2*s/ (t*s+l)A2 
Gi  =  (2*t*s+l) / (t*s+l)A2 

controller  design. m  file 

This  script  is  run  when  the  Qball-X4  model  is 

initialized  and  calculates  the  controller  parameters. 

%  PITCH  and  ROLL 
wnfom  =  15; 

L  =  0.2; 
w  =  15; 

K  =  12  0; 

J  =  0.03; 

J  yaw  =  0.04; 

CLimit  =  0.025; 

M  =  1.4; 
g  =  9.8; 

Am  =[010 

0  0  K*L/J 
0  0  -w]  ; 

Bm  =  [0  0  w  ]  '  ; 

Aobs  =  Am'  ; 

Bobs  =  eye ( 3 ) ; 

Qobs  =  diag([.001  10000  .01]); 

Robs  =  diag ( [  1  1  1  ])*1; 

Kobs  =  lqr (Aobs , Bobs , Qobs , Robs ) 

Kobs  =  Kobs' ; 

Aobs  =  Aobs' -Kobs*Bobs' ; 
eig (Aobs ) 

Bobs  =  [Bm  Kobs] 

Cobs  =  eye ( 3 ) 

Dobs  =  [0000 
0  0  0  0 
0  0  0  0  ]  ; 
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%  augment  with  integrator 
Ai  =  [Am  [0  0  0  ] ' 

1000]; 

Bi  =  [Bm'  0 ] '  ; 

Ci  =  eye (4); 

Di  =  [0  0  0  0  ]  '  ; 

Q  =  diag ( [ 100  0  22000  10]); 

R  =  30000; 

ki  =  lqr (Ai , Bi , Q,  R)  ; 
rp  eig  =  eig (Ai-Bi*ki) ; 

fprintf  ( '************************************************  \n ' ) ; 
fprintf ( 'ROLL,  PITCH  DESIGN  \n'  ) ; 

fprintf (  'P  =  %5.3f  D  =  %5.3f  Actuator  =  %5.3f  I  =  %5.3f  \n\n',ki(l), 
ki  (2)  ,  ki  (3)  ,  ki  (4)  )  ; 
for  i  =  1 : 4 

fprintf ( '  %5.3f  +  %5.3f  i  \n  ' , real ( rp_eig ( i ) )  ,  imag (rp_eig (i) ) ) ; 

end; 


%POSITION  CONTROLLER  (C2) 

%  XZ  travel 
xyposition  =  1 

tlimit  =  5*pi/180;  %max  pitch  cmd  radians 
%tlimit  =  15*pi/180;  %max  pitch  cmd  radians 
vlimit  =  0.3;  %  max  speed  cmd  in  m/sec 
%vlimit  =  0.5;  %  max  speed  cmd  in  m/sec 

Tau  theta  =  1/7;  %  closed  loop  time  constant  for  pitch  response 
wt  =1/Tau  theta;  %closed  loop  theta  bandwidth 
kt  =  1 ; 
a  =  [0  1  0  0 
0  0  g  0 
0  0  -wt  0 
1000]; 
b  =  [ 0  0  wt  0  ] ' ; 

q  =  diag ( [  5  2  0  0.1]); 
r  =  50; 

k  =  lqr (a, b, q, r) ; 

ac  =  a-b*k; 

xy_eig  =  eig(a-b*k); 

Kp  =  k ( 1 ) ; 

Kd  =  k ( 2  )  ; 

Ki  =  k ( 4 ) ; 

Kw  =  k ( 3 ) ; 

fprintf (' \n\n  X  Y  Design  \n' ) ; 

fprintf (  'P  =  %5.3f  D  =  %5.3f  Actuator  =  %5.3f  I  =  %5.3f  \n\n',k(l), 
k (2)  ,  k (3)  ,  k (4) )  ; 
for  i  =  1 : 4 

fprintf ( '  %5.3f  +  %5.3f  i  \n  ' , real (xy_eig ( i )  )  ,  imag (xy_eig ( i )  )  )  ; 

end; 
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Z  axis  without  actuator 


vlimith  =  0.1; 

Amh  =  [01 
0  0  ] 

Bmh  =  [0  4 *K/M] ' ; 

Cmh  =  [1  0 ] ; 

Dmh  =  0 ; 

%  augment  with  integrator 
Aih  =  [Amh  [0  0  ] ' 

1  0  0]; 

Bih  =  [Bmh'  0 ] ' ; 

Cih  =  eye  (3) ; 

Dih  =  [0  0  0] '  ; 

Q  =  diag  (  [1  0  50] )  ; 

R  =  5000000; 

kh  =  lqr (Aih, Bih, Q, R) ; 

h  eig  =  eig (Aih-Bih*kh) ; 

fprintf ( ' Z  DESIGN  \n'  )  ; 

fprintf (  'P  =  %5.3f  D  =  %5.3f  I  =  %5.3f  \n\n',kh(l),  kh(2),kh(3)) 
for  i  =  1 : 3 

fprintf ( '  %5.3f  +  %5.3f  i  \n  ' ,  real (h_eig (i) )  ,  imag (h_eig ( i ) ) ) ; 

end; 

Kph  =  kh  ( 1 )  ; 

Kdh  =  kh ( 2 ) ; 

Kwh  =  0 ; 

Kih  =  kh (3 ) ; 

%  yaw  axis 
yaw  =  1 
Ky  =  4; 

J  y  =  0.04; 

Amy  =  [01 
0  0]; 

Bmy  =  [ 0  Ky/ Jy] ' ; 

Cmy  =  eye (2 ) ; 

Dmy  =  [ 0 ;  0  ]  ; 

Qy  =  diag ( [1  0.1]); 

Ry  =  1000; 

ky  =  lqr (Amy, Bmy, Qy,  Ry) 
h  eigy  =  eig (Amy-Bmy*ky) 

Kpyaw  =  ky ( 1 ) 

Kdyaw  =  ky(2) 


Bih  =  [Bih, [0  1  0] ' ] ; 
Dih  =  [Dih,  [0  0  0]  '  ]  ; 
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DMlopt.m  file 

This  script  uses  the  fminsearch  and  DMlfun  function  to 
run  the  Simulink  model  to  minimize  the  discrepancy 
function . 


%%  Initial  guesses 
x0= [20/1000  % 

0.08  % 

5/10  % 

0.08  % 

-5/10  % 

-0.02  % 

-0.02];  % 


for  varied  parameters 
tauf 

X0  tpl_prime 
dirXO  tpl  prime 
Xf  tpl_prime 
dirXf  tpl  prime 
lamO  2pr 
lamf  2pr 


%%  Optimization 
t  =  cputime; 

options=optimset ( 'TolFun' ,  le-1,  'TolX' ,  le-1,  'Display',  'final'); 

[xO, fval, exitflag] =fminsearch (@DMlfun,  xO) 
time  elapsed  =  cputime-t 


%%  Optimal  values 

tauf  = 
X0  tpl_prime  = 
dirXO  tpl  prime  = 
Xf  tpl_prime  = 
dirXf  tpl  prime  = 
lamO  2pr  = 
lamf  2pr  = 


of  varied  parameters 

xO  (1)  ; 
xO  (2)  ; 
xO  (3)  ; 
xO  (4)  ; 
xO  (5)  ; 
xO (6) ; 
xO  (7)  ; 


DMlfun. m  function  file 

This  function  runs  the  Simulink  file  "DM2_1"  using  the 
varied  parameters  given  as  the  input  to  the  function. 

function  f  =  DMlfun (xO) 
tauf  =  xO ( 1 ) ; 

X0  tpl  prime  =  x0(2); 
dirXO  tpl  prime  =  x0(3); 

Xf  tpl  prime  =  x0(4); 
dirXf  tpl  prime  =  x0(5); 
lam0_2pr  =  x0(6); 

lamf  2pr  =  x0(7); 

opt  =  simset ( 'SrcWorkspace' ,  'Current'); 
sim('DM2_l',  [0  200],  opt); 
f=yout (length (yout)  )  ; 


64 


Compute  Controls. m  file 

This  file  calculates  the  position  commands  for  the 
quadrotor  at  the  controller  frequency. 

%  Controller  speed 
ctrl_t_step  =  .005; 

%  Run  Simulation  to  get  data 
sim ( 'DM3_1' ,  [0  200] ) 

[m_a,n_a]  =  size  (a); 

t_a_end  =  a(m_a,l); 

t_a  =  0 : ctrl_t_step : t_a_end; 

[m_b,n_b]  =  size(b); 

t_b_end  =  b(m_b,l); 

t_b  =  0 : ctrl_t_step : t_b_end; 

%  Setup  Variables 


tau 

a 

=  a  ( : ,  1 

x  a 

- 

a  (  :  ,  4  )  ; 

y_a 

= 

a  ( : ,  5 )  ; 

z  a 

= 

a  (  : ,  6 )  ; 

tau 

b 

\ — 1 

XI 

II 

x  b 

"= 

b  ( : ,  4 )  ; 

y_b 

= 

b  (  : ,  5 )  ; 

z  b 

= 

b  (  : ,  6 )  ; 

%%  Interpolate  data 

%  Interpolate  data  between  points  at  the  same  frequency  the  controller 
%  runs  at. 

x_a  =  interpl (tau_a, x_a, t_a, ' linear'  ) ; 
y_a  =  interpl (tau_a, y_a, t_a, ' linear'  ) ; 
z_a  =  interpl (tau_a, z_a, t_a, ' linear'  )  ; 

x  b  =  interpl (tau  b,x  b,t  b, ' linear'); 
y_b  =  interpl (tau_b, y_b, t_b, ' linear' ) ; 
z_b  =  interpl (tau_b, z_b, t_b, ' linear'  ) ; 

%%  Setup  data  for  use  in  controller 

%  Setup  a  series  of  commands  for  the  first  waypoint 
t  start  =  20;  %Start  time  for  maneuver 
t_a  =  t_a+t_start; 
t_b  =  t_b+t_start; 

t_beginning  =  0 : ctrl_t_step : t_start-ctrl_t_step; 
z  comp  =  ones ( 1 , length (t  beginning)); 

t  comp  a  =  [t  beginning'  t  beginning' ;t  a'  t  a' ] ; 
x  command  a  =  [t  beginning'  x  a(l)*z  comp';t  a'  x  a' ] ; 
y_command_a  =  [t_beginning'  y_a ( 1 ) * z_comp' ; t_a'  y_a' ] ; 
z  command  a  =  [t  beginning'  z  a(l)*z  comp';t  a'  z  a' ] ; 
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t_comp_b  = 
x  command  b 
y  command  b 
z  command  b 


[t 


beginning'  t 
[t  beginning' 
[t  beginning' 
[t  beginning' 


beginning' 
x  b  ( 1 )  *  z 
y_b ( 1 ) *  z 
z  b  ( 1 )  *  z 


; t_b '  t_b '  ]  ; 
comp';t_b'  x_b' ] 
comp';t_b'  y_b' ] 
comp';t_b'  z_b' ] 
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